/*
 * @Author: Yicheng Chen - BUAA
 * @Date: 2022-07-29 21:27:29
 * @LastEditTime: 2022-08-01 20:25:17
 * @FilePath: /hw4/src/grid_path_searcher/include/hw_tool.h
 */
#ifndef _HW_TOOL_H_
#define _HW_TOOL_H_

#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <Eigen/Eigen>
#include "backward.hpp"
#include "math.h"
#include <State.h>

class Homeworktool
{
private:
protected:
    uint8_t *data;

    int GLX_SIZE, GLY_SIZE, GLZ_SIZE;
    int GLXYZ_SIZE, GLYZ_SIZE;

    double resolution, inv_resolution;
    double gl_xl, gl_yl, gl_zl;
    double gl_xu, gl_yu, gl_zu;

    Eigen::Vector3d gridIndex2coord(const Eigen::Vector3i &index);
    Eigen::Vector3i coord2gridIndex(const Eigen::Vector3d &pt);

public:
    Homeworktool(){};
    ~Homeworktool(){};

    void initGridMap(double _resolution, Eigen::Vector3d global_xyz_l, Eigen::Vector3d global_xyz_u, int max_x_id, int max_y_id, int max_z_id);
    void setObs(const double coord_x, const double coord_y, const double coord_z);
    bool isObsFree(const double coord_x, const double coord_y, const double coord_z);

    Eigen::Vector3d coordRounding(const Eigen::Vector3d &coord);
    double OptimalBVP(Eigen::Vector3d _start_position, Eigen::Vector3d _start_velocity, Eigen::Vector3d _target_position);
    double getJ(double T, Eigen::Vector3d _start_position, Eigen::Vector3d _start_velocity, Eigen::Vector3d _target_position);
};

#endif